/**
 *******************************************************************************
 * @file  main.c
 * @brief Main program.
 @verbatim
   Change Logs:
   Date             Author          Notes
   2025-05-05       CDT             First version
 @endverbatim
 *******************************************************************************
 * Copyright (C) 2022, Xiaohua Semiconductor Co., Ltd. All rights reserved.
 *
 * This software component is licensed by XHSC under BSD 3-Clause license
 * (the "License"); You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *                    opensource.org/licenses/BSD-3-Clause
 *
 *******************************************************************************
 */

/*******************************************************************************
 * Include files
 ******************************************************************************/
#include "main.h"
/*******************************************************************************
 * Local type definitions ('typedef')
 ******************************************************************************/

/*******************************************************************************
 * Local pre-processor symbols/macros ('#define')
 ******************************************************************************/
/*******************************************************************************
 * Global variable definitions (declared in header file with 'extern')
 ******************************************************************************/

/*******************************************************************************
 * Local function prototypes ('static')
 ******************************************************************************/
/* INT_SRC_TMRA_2_OVF Callback. */
static void INT_SRC_TMRA_1_OVF_IrqCallback(void);
/* INT_SRC_TMRA_2_OVF Callback. */
static void INT_SRC_TMRA_2_OVF_IrqCallback(void);
/* Configures TimerA. */
static void App_TimerACfg(void);
/* Configures USARTx. */
static void App_USARTxCfg(void);
/*******************************************************************************
 * Local variable definitions ('static')
 ******************************************************************************/

/*******************************************************************************
 * Function implementation - global ('extern') and local ('static')
 ******************************************************************************/
// Clock Config
static void App_ClkCfg(void)
{
    /* Set bus clock div. */
    CLK_SetClockDiv(CLK_BUS_CLK_ALL, (CLK_HCLK_DIV1 | CLK_EXCLK_DIV2 | CLK_PCLK0_DIV1 | CLK_PCLK1_DIV2 | \
                                   CLK_PCLK2_DIV4 | CLK_PCLK3_DIV4 | CLK_PCLK4_DIV2));
    /* sram init include read/write wait cycle setting */
    SRAM_SetWaitCycle(SRAM_SRAM_ALL, SRAM_WAIT_CYCLE1, SRAM_WAIT_CYCLE1);
    SRAM_SetWaitCycle(SRAM_SRAMH, SRAM_WAIT_CYCLE0, SRAM_WAIT_CYCLE0);
    /* flash read wait cycle setting */
    EFM_SetWaitCycle(EFM_WAIT_CYCLE5);
    /* XTAL config */
    stc_clock_xtal_init_t stcXtalInit;
    (void)CLK_XtalStructInit(&stcXtalInit);
    stcXtalInit.u8State = CLK_XTAL_ON;
    stcXtalInit.u8Drv = CLK_XTAL_DRV_HIGH;
    stcXtalInit.u8Mode = CLK_XTAL_MD_OSC;
    stcXtalInit.u8StableTime = CLK_XTAL_STB_2MS;
    (void)CLK_XtalInit(&stcXtalInit);
    /* MPLL config */
    stc_clock_pll_init_t stcMPLLInit;
    (void)CLK_PLLStructInit(&stcMPLLInit);
    stcMPLLInit.PLLCFGR = 0UL;
    stcMPLLInit.PLLCFGR_f.PLLM = (1UL - 1UL);
    stcMPLLInit.PLLCFGR_f.PLLN = (50UL - 1UL);
    stcMPLLInit.PLLCFGR_f.PLLP = (2UL - 1UL);
    stcMPLLInit.PLLCFGR_f.PLLQ = (2UL - 1UL);
    stcMPLLInit.PLLCFGR_f.PLLR = (2UL - 1UL);
    stcMPLLInit.u8PLLState = CLK_PLL_ON;
    stcMPLLInit.PLLCFGR_f.PLLSRC = CLK_PLL_SRC_XTAL;
    (void)CLK_PLLInit(&stcMPLLInit);
    /* 3 cycles for 126MHz ~ 200MHz */
    GPIO_SetReadWaitCycle(GPIO_RD_WAIT3);
    /* Switch driver ability */
    PWC_HighSpeedToHighPerformance();
    /* Set the system clock source */
    CLK_SetSysClockSrc(CLK_SYSCLK_SRC_PLL);
}

// Port Config
static void App_PortCfg(void)
{
    /* GPIO initialize */
    stc_gpio_init_t stcGpioInit;
    /* PB0 set to GPIO-Output */
    (void)GPIO_StructInit(&stcGpioInit);
    stcGpioInit.u16PinDir = PIN_DIR_OUT;
    stcGpioInit.u16PinAttr = PIN_ATTR_DIGITAL;
    (void)GPIO_Init(GPIO_PORT_B, GPIO_PIN_00, &stcGpioInit);
	
		(void)GPIO_StructInit(&stcGpioInit);
    stcGpioInit.u16PinDir = PIN_DIR_OUT;
    stcGpioInit.u16PinAttr = PIN_ATTR_DIGITAL;
		stcGpioInit.u16PinOutputType = PIN_OUT_TYPE_CMOS;
//		stcGpioInit.u16PullUp = PIN_PU_ON;
    (void)GPIO_Init(GPIO_PORT_A, GPIO_PIN_03, &stcGpioInit);
		
		(void)GPIO_StructInit(&stcGpioInit);
    stcGpioInit.u16PinDir = PIN_DIR_OUT;
    stcGpioInit.u16PinAttr = PIN_ATTR_DIGITAL;
		stcGpioInit.u16PinOutputType = PIN_OUT_TYPE_NMOS;
//		stcGpioInit.u16PullUp = PIN_PU_ON;
    (void)GPIO_Init(GPIO_PORT_A, GPIO_PIN_04, &stcGpioInit);

    GPIO_SetFunc(GPIO_PORT_A, GPIO_PIN_00, GPIO_FUNC_4); // TIMA-2-PWM1/TIMA-2-CLKA

    GPIO_SetFunc(GPIO_PORT_A, GPIO_PIN_01, GPIO_FUNC_4); // TIMA-2-PWM2/TIMA-2-CLKB

    GPIO_SetFunc(GPIO_PORT_A, GPIO_PIN_02, GPIO_FUNC_4); // TIMA-2-PWM3

    GPIO_SetFunc(GPIO_PORT_A, GPIO_PIN_09, GPIO_FUNC_32); // USART1-TX
		
	
}

// Int Config
static void App_IntCfg(void)
{
    stc_irq_signin_config_t stcIrq;

    /* IRQ sign-in */
    stcIrq.enIntSrc = INT_SRC_TMRA_2_OVF;
    stcIrq.enIRQn = INT080_IRQn;
    stcIrq.pfnCallback = &INT_SRC_TMRA_2_OVF_IrqCallback;
    (void)INTC_IrqSignIn(&stcIrq);
    /* NVIC config */
    NVIC_ClearPendingIRQ(INT080_IRQn);
    NVIC_SetPriority(INT080_IRQn, DDL_IRQ_PRIO_15);
    NVIC_EnableIRQ(INT080_IRQn);
		
    /* IRQ sign-in */
    stcIrq.enIntSrc = INT_SRC_TMRA_1_OVF;
    stcIrq.enIRQn = INT081_IRQn;
    stcIrq.pfnCallback = &INT_SRC_TMRA_1_OVF_IrqCallback;
    (void)INTC_IrqSignIn(&stcIrq);
    /* NVIC config */
    NVIC_ClearPendingIRQ(INT081_IRQn);
    NVIC_SetPriority(INT081_IRQn, DDL_IRQ_PRIO_15);
    NVIC_EnableIRQ(INT081_IRQn);
}

/**
 * @brief  Main function of the project
 * @param  None
 * @retval int32_t return value, if needed
 */
#define PI (3.1415926f)
uint16_t u16Polarity[3] = {50, 50, 0};
#include <math.h>
#include <string.h>
// 角度单位：弧度（如需角度制，需乘以π/180）
// 三角函数使用标准库，如需更高性能可替换为查表法

/**
 * Clarke逆变换：将αβ轴分量转换为三相ABC分量
 * @param alpha α轴分量
 * @param beta  β轴分量
 * @param a     输出A相分量
 * @param b     输出B相分量
 * @param c     输出C相分量
 */
void inverse_clarke(float alpha, float beta, float *a, float *b, float *c)
{
    *a = alpha;
    *b = -0.5f * alpha + 0.8660254f * beta; // 0.8660254 = sqrt(3)/2
    *c = -0.5f * alpha - 0.8660254f * beta;
}

/**
 * Park逆变换：将dq轴分量转换为αβ轴分量
 * @param d     d轴分量
 * @param q     q轴分量
 * @param theta 电角度（弧度）
 * @param alpha 输出α轴分量
 * @param beta  输出β轴分量
 */
void inverse_park(float d, float q, float theta, float *alpha, float *beta)
{
    float sin_val = sinf(theta);
    float cos_val = cosf(theta);

    *alpha = d * cos_val - q * sin_val;
    *beta = d * sin_val + q * cos_val;
}

// 假设dq轴电压指令和当前电角度
float Vd = 0.0f;    // d轴电压指令
float Vq = 50.0f;    // q轴电压指令
float theta = 0.0f; // 电角度（π/2弧度 = 90°）

float Valpha, Vbeta;
float Va, Vb, Vc;

#include "as5600.h"
#include "soft_i2c.h"

#define IIC_SCL_GPIO_Port GPIO_PORT_A
#define IIC_SDA_GPIO_Port GPIO_PORT_A
#define IIC_SCL_Pin GPIO_PIN_03
#define IIC_SDA_Pin GPIO_PIN_04
void scl_high(void){GPIO_SetPins(IIC_SCL_GPIO_Port,IIC_SCL_Pin);}
void scl_low(void) {GPIO_ResetPins(IIC_SCL_GPIO_Port,IIC_SCL_Pin);}
void sda_high(void){GPIO_SetPins(IIC_SDA_GPIO_Port,IIC_SDA_Pin);}
void sda_low(void) {GPIO_ResetPins(IIC_SDA_GPIO_Port,IIC_SDA_Pin);}
uint8_t scl_read(void){return GPIO_ReadInputPins(IIC_SCL_GPIO_Port,IIC_SCL_Pin);}
uint8_t sda_read(void){return GPIO_ReadInputPins(IIC_SDA_GPIO_Port,IIC_SDA_Pin);}
void delay_nop(uint32_t x)
{
	volatile uint32_t i;
	for(i = 0;i < x;i++){
		__NOP();
	}
}

i2c_ops_t i2c_ops;
as5600_data_t as5600_data;

int32_t main(void)
{
    /* Register write unprotected for some required peripherals. */
    LL_PERIPH_WE(LL_PERIPH_ALL);
    // Clock Config
    App_ClkCfg();
    // Port Config
    App_PortCfg();
    // Int Config
    App_IntCfg();
    // TimerA Config
    App_TimerACfg();
    // USARTx Config
    App_USARTxCfg();
    /* Register write protected for some required peripherals. */
    LL_PERIPH_WP(LL_PERIPH_ALL);
	
		i2c_ops.scl_high = scl_high;
		i2c_ops.scl_low  = scl_low;
		i2c_ops.sda_high = sda_high;
		i2c_ops.sda_low  = sda_low;
		i2c_ops.sda_read = sda_read;
		i2c_ops.delay    = delay_nop;
		i2c_ops.delay_x  = 1;
		
		/* Start timerA */
    TMRA_Start(CM_TMRA_1);
		
    for (;;)
    {
//			soft_i2c_readnByte(&i2c_ops,AS5600_ADDRESS_MAG,_stat,as5600_data.read_data,1);
//			as5600_status_analytical(&as5600_data);
//			DDL_DelayMS(10);
//        // Park逆变换：dq → αβ
//        inverse_park(Vd, Vq, theta, &Valpha, &Vbeta);
//        // Clarke逆变换：αβ → ABC
//        inverse_clarke(Valpha, Vbeta, &Va, &Vb, &Vc);
//			
//				u16Polarity[0] = (uint16_t)(Va + 50.0f);
//				u16Polarity[1] = (uint16_t)(Vb + 50.0f);
//				u16Polarity[2] = (uint16_t)(Vc + 50.0f);
//	
//        if (u16Polarity[0] == 0)
//            CM_TMRA_2->PCONR1 &= ~(0x01 << 12);
//        else
//            CM_TMRA_2->PCONR1 |= 0x01 << 12;
//        if (u16Polarity[1] == 0)
//            CM_TMRA_2->PCONR2 &= ~(0x01 << 12);
//        else
//            CM_TMRA_2->PCONR2 |= 0x01 << 12;
//        if (u16Polarity[2] == 0)
//            CM_TMRA_2->PCONR3 &= ~(0x01 << 12);
//        else
//            CM_TMRA_2->PCONR3 |= 0x01 << 12;

//        CM_TMRA_2->CMPAR1 = (uint16_t)((float)u16Polarity[0] * 0.01 * (float)CM_TMRA_2->PERAR);
//        CM_TMRA_2->CMPAR2 = (uint16_t)((float)u16Polarity[1] * 0.01 * (float)CM_TMRA_2->PERAR);
//        CM_TMRA_2->CMPAR3 = (uint16_t)((float)u16Polarity[2] * 0.01 * (float)CM_TMRA_2->PERAR);
    }
}

float kj = 0.01;

/* INT_SRC_TMRA_2_OVF Callback. */
static void INT_SRC_TMRA_2_OVF_IrqCallback(void)
{
    // add your codes here
    //	GPIO_TogglePins(GPIO_PORT_B, GPIO_PIN_00);
}
char str[128];
/* INT_SRC_TMRA_1_OVF Callback. */
static void INT_SRC_TMRA_1_OVF_IrqCallback(void)
{
    //add your codes here
		soft_i2c_readnByte(&i2c_ops,AS5600_ADDRESS_MAG,_stat,as5600_data.read_data,1);
		as5600_status_analytical(&as5600_data);
		soft_i2c_readnByte(&i2c_ops,AS5600_ADDRESS_MAG,_ang_hi,as5600_data.read_data,2);
		as5600_angle_analytical(&as5600_data);
	
//	theta+=kj;
//	theta = fmod(theta,2*PI);
//	GPIO_TogglePins(GPIO_PORT_B, GPIO_PIN_00);
	
//	sprintf(str,"%d,%d,%d\n",u16Polarity[0],u16Polarity[1],u16Polarity[2]);
	
	sprintf(str,"%f\n",as5600_data.angle);
	USART_UART_Trans(CM_USART1,str,strlen(str),0xffff);
}

// TimerA Config
static void App_TimerACfg(void)
{
    stc_tmra_init_t stcTmraInit;
    stc_tmra_pwm_init_t stcPwmInit;

    /* Enable TMRA_2 peripheral clock */
    FCG_Fcg2PeriphClockCmd(FCG2_PERIPH_TMRA_2, ENABLE);

    /************************* Configure TMRA_2 counter *************************/
    (void)TMRA_StructInit(&stcTmraInit);
    /* Config software count */
    stcTmraInit.sw_count.u8ClockDiv = TMRA_CLK_DIV8;
    stcTmraInit.sw_count.u8CountMode = TMRA_MD_SAWTOOTH;
    stcTmraInit.sw_count.u8CountDir = TMRA_DIR_UP;
    stcTmraInit.u32PeriodValue = 625 - 1;
    (void)TMRA_Init(CM_TMRA_2, &stcTmraInit);

    /* Enable overflow interrupt */
    TMRA_IntCmd(CM_TMRA_2, TMRA_INT_OVF, ENABLE);

    /************************* Configure TMRA_2_1 CMP ***************************/
    /* Config PWM output */
    (void)TMRA_PWM_StructInit(&stcPwmInit);
    stcPwmInit.u32CompareValue = 0;
    stcPwmInit.u16StartPolarity = TMRA_PWM_HOLD;
    stcPwmInit.u16StopPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16CompareMatchPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16PeriodMatchPolarity = TMRA_PWM_HIGH;
    (void)TMRA_PWM_Init(CM_TMRA_2, TMRA_CH1, &stcPwmInit);
    /* PWM pin function set */
    TMRA_SetFunc(CM_TMRA_2, TMRA_CH1, TMRA_FUNC_CMP);
    /* PWM output command */
    TMRA_PWM_OutputCmd(CM_TMRA_2, TMRA_CH1, ENABLE);

    /************************* Configure TMRA_2_2 CMP ***************************/
    /* Config PWM output */
    (void)TMRA_PWM_StructInit(&stcPwmInit);
    stcPwmInit.u32CompareValue = 0;
    stcPwmInit.u16StartPolarity = TMRA_PWM_HOLD;
    stcPwmInit.u16StopPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16CompareMatchPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16PeriodMatchPolarity = TMRA_PWM_HIGH;
    (void)TMRA_PWM_Init(CM_TMRA_2, TMRA_CH2, &stcPwmInit);
    /* PWM pin function set */
    TMRA_SetFunc(CM_TMRA_2, TMRA_CH2, TMRA_FUNC_CMP);
    /* PWM output command */
    TMRA_PWM_OutputCmd(CM_TMRA_2, TMRA_CH2, ENABLE);

    /************************* Configure TMRA_2_3 CMP ***************************/
    /* Config PWM output */
    (void)TMRA_PWM_StructInit(&stcPwmInit);
    stcPwmInit.u32CompareValue = 0;
    stcPwmInit.u16StartPolarity = TMRA_PWM_HOLD;
    stcPwmInit.u16StopPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16CompareMatchPolarity = TMRA_PWM_LOW;
    stcPwmInit.u16PeriodMatchPolarity = TMRA_PWM_HIGH;
    (void)TMRA_PWM_Init(CM_TMRA_2, TMRA_CH3, &stcPwmInit);
    /* PWM pin function set */
    TMRA_SetFunc(CM_TMRA_2, TMRA_CH3, TMRA_FUNC_CMP);
    /* PWM output command */
    TMRA_PWM_OutputCmd(CM_TMRA_2, TMRA_CH3, ENABLE);

    CM_TMRA_2->PCONR1 &= ~(0x01 << 12);
    CM_TMRA_2->PCONR2 &= ~(0x01 << 12);
    CM_TMRA_2->PCONR3 &= ~(0x01 << 12);

    /* Start timerA */
    TMRA_Start(CM_TMRA_2);
		
		/* Enable TMRA_1 peripheral clock */
    FCG_Fcg2PeriphClockCmd(FCG2_PERIPH_TMRA_1, ENABLE);

    /************************* Configure TMRA_1 counter *************************/
    (void)TMRA_StructInit(&stcTmraInit);
    /* Config software count */
    stcTmraInit.sw_count.u8ClockDiv = TMRA_CLK_DIV32;
    stcTmraInit.sw_count.u8CountMode = TMRA_MD_SAWTOOTH;
    stcTmraInit.sw_count.u8CountDir = TMRA_DIR_UP;
    stcTmraInit.u32PeriodValue = 31240;
    (void)TMRA_Init(CM_TMRA_1, &stcTmraInit);

    /* Enable overflow interrupt */
    TMRA_IntCmd(CM_TMRA_1, TMRA_INT_OVF, ENABLE);

}

// USARTx Config
static void App_USARTxCfg(void)
{
    stc_usart_uart_init_t stcUartInit;

    /* Enable USART1 clock */
    FCG_Fcg1PeriphClockCmd(FCG1_PERIPH_USART1, ENABLE);
    /************************* Configure USART1***************************/
    USART_DeInit(CM_USART1);
    (void)USART_UART_StructInit(&stcUartInit);
    stcUartInit.u32ClockSrc = USART_CLK_SRC_INTERNCLK;
    stcUartInit.u32ClockDiv = USART_CLK_DIV1;
    stcUartInit.u32CKOutput = USART_CK_OUTPUT_DISABLE;
    stcUartInit.u32Baudrate = 1152000UL;
    stcUartInit.u32DataWidth = USART_DATA_WIDTH_8BIT;
    stcUartInit.u32StopBit = USART_STOPBIT_1BIT;
    stcUartInit.u32Parity = USART_PARITY_NONE;
    stcUartInit.u32OverSampleBit = USART_OVER_SAMPLE_16BIT;
    stcUartInit.u32FirstBit = USART_FIRST_BIT_LSB;
    stcUartInit.u32StartBitPolarity = USART_START_BIT_FALLING;
    stcUartInit.u32HWFlowControl = USART_HW_FLOWCTRL_RTS;
    USART_UART_Init(CM_USART1, &stcUartInit, NULL);
    /* Enable USART_TX | USART_RX function */
    USART_FuncCmd(CM_USART1, (USART_TX | USART_RX), ENABLE);
}

/**
 * @}
 */

/**
 * @}
 */

/*******************************************************************************
 * EOF (not truncated)
 ******************************************************************************/
